/**
 * @file bmi160.c
 * @author yuchenghao (yuchenghao@mail.ncut.edu.cn)
 * @brief
 * @version 0.1
 * @date 2024-04-05
 *
 * @copyright Copyright (c) 2024
 *
 */
#include "bmi160.h"
#include "bsp_bmi160.h"
#include "math.h"
#include "MadgwickAHRS.h"
#define GYRO_RANGE_DPS 2000.0f     // 陀螺仪量程 (dps)
#define ACCEL_RANGE_G 16.0f        // 加速度计量程 (G)
#define SAMPLE_PERIOD 0.05f        // 采样周期，单位为秒
#define halfT SAMPLE_PERIOD / 2.0f // 采样周期的一半
#define eular_Kp 0.0f              // 比例系数
#define eular_Ki 0.1f              // 积分系数

// 宏定义转换函数
#define RAW_TO_DPS(raw) ((float)(raw) * GYRO_RANGE_DPS / 32768.0f)
#define RAW_TO_G(raw) (((float)(raw) * ACCEL_RANGE_G / 32768.0f) * 9.8015) // 这个9.8根据当地的重力加速度改
#define RtA 57.2957795130823208125                                         // 弧度转换为角度

typedef struct
{
    float lpf_prev_output;
    float alpha; // 滤波器的参数，可以根据需要调整
    uint8_t lpf_initialized; // 标志变量，用于初始化滤波器
}lpf_t;

struct bmi160_dev sensor;
float update_lowPassFilter(float input,lpf_t *lpf);
/**
 * @brief 初始化BMI160传感器，由于名称被官方代码占用，所以加入了usr字符
 *
 */
uint8_t usr_bmi160_init(void)
{
    sensor.id = BMI160_I2C_ADDR;
    // sensor.interface = BMI160_I2C_INTF;
    sensor.read = port_i2c_read;
    sensor.write = port_i2c_write;
    sensor.delay_ms = HAL_Delay;
    int8_t rslt;
    rslt = bmi160_init(&sensor);
    sensor.accel_cfg.odr = BMI160_ACCEL_ODR_1600HZ;
    sensor.accel_cfg.range = BMI160_ACCEL_RANGE_16G;
    sensor.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4;

    sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE; // 加速度计的电源模式

    /* Select the Output data rate, range of Gyroscope sensor */
    sensor.gyro_cfg.odr = BMI160_GYRO_ODR_1600HZ;       // 陀螺仪输出速率
    sensor.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS; // 陀螺仪输出范围
    sensor.gyro_cfg.bw = BMI160_GYRO_BW_OSR4_MODE;      // 陀螺仪输出模式

    /* Select the power mode of Gyroscope sensor */
    sensor.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE; // 陀螺仪电源模式

    /* Set the sensor configuration */
    rslt = bmi160_set_sens_conf(&sensor);
    Madgwick_init();
    if (rslt == BMI160_OK)
    {
        return 1;
    }
    return 0;
}

lpf_t acc_x_lpf = {0.0f, 0.9f, 0};
lpf_t acc_y_lpf = {0.0f, 0.9f, 0};
lpf_t acc_z_lpf = {0.0f, 0.9f, 0};

lpf_t gyro_x_lpf = {0.0f, 0.5f, 1};
lpf_t gyro_y_lpf = {0.0f, 0.5f, 1};
lpf_t gyro_z_lpf = {0.0f, 0.5f, 1};




/**
 * @brief BMI160传感器更新数据
 *
 * @param data 数据结构体
 * @return uint8_t 成功标志
 */
uint8_t bmi160_update(imu_data_t *data)
{
    struct bmi160_sensor_data bmi160_accel;
    struct bmi160_sensor_data bmi160_gyro;
    if (bmi160_get_sensor_data(BMI160_ACCEL_SEL | BMI160_GYRO_SEL, &bmi160_accel, &bmi160_gyro, &sensor) != BMI160_OK)
    {
        return bmi160_get_sensor_data(BMI160_ACCEL_SEL | BMI160_GYRO_SEL, &bmi160_accel, &bmi160_gyro, &sensor);
    }
    data->accel_x_raw = bmi160_accel.x;
    data->accel_y_raw = bmi160_accel.y;
    data->accel_z_raw = bmi160_accel.z;

    data->gyro_x_raw = bmi160_gyro.x;
    data->gyro_y_raw = bmi160_gyro.y;
    data->gyro_z_raw = bmi160_gyro.z;
    data->updatetime = bmi160_accel.sensortime;
    data->accel_x = RAW_TO_G(data->accel_x_raw);
    data->accel_y = RAW_TO_G(data->accel_y_raw);
    data->accel_z = RAW_TO_G(data->accel_z_raw);
    data->gyro_x = RAW_TO_DPS(data->gyro_x_raw);
    data->gyro_y = RAW_TO_DPS(data->gyro_y_raw);
    data->gyro_z = RAW_TO_DPS(data->gyro_z_raw);

    // //为每个陀螺仪原始数据加入低通滤波器
    // data->accel_x = update_lowPassFilter(data->accel_x,&acc_x_lpf);
    // data->accel_y = update_lowPassFilter(data->accel_y,&acc_y_lpf);
    // data->accel_z = update_lowPassFilter(data->accel_z,&acc_z_lpf);
    // data->gyro_x = update_lowPassFilter(data->gyro_x,&gyro_x_lpf);
    // data->gyro_y = update_lowPassFilter(data->gyro_y,&gyro_y_lpf);
    // data->gyro_z = update_lowPassFilter(data->gyro_z,&gyro_z_lpf);

    return 1;
}
void complementaryFilter(imu_data_t *data);

/**
 * @brief 更新低通滤波器
 *
 * @param input 输入数据
 * @return float 输出数据
 */


float update_lowPassFilter(float input,lpf_t *lpf)
{
    if (!lpf->lpf_initialized)
    {
        lpf->lpf_prev_output = input;
        lpf->lpf_initialized = 1;
    }

    lpf->lpf_prev_output = lpf->alpha * input + (1.0f - lpf->alpha) * lpf->lpf_prev_output;
    return lpf->lpf_prev_output;
}

/**
 * @brief 更新欧拉角数据
 *
 * @param data
 * @return uint8_t
 */
uint8_t eular_update(imu_data_t *data)
{
    if(HAL_GetTick() < 1000)
    {
        return 0;//等待陀螺仪初始化完成
    }
    // complementaryFilter(data);
    // data->yaw = update_lowPassFilter(data->yaw);
    // data->yaw = (-0.0001f * HAL_GetTick()) + data->yaw;//积分器修正
    // Angle_Calculate(data);
    Madgwick_updateIMU(data->gyro_x, data->gyro_y, data->gyro_z, data->accel_x, data->accel_y, data->accel_z);
    data->roll = getRoll();
    data->pitch = getPitch();
    data->yaw = getYaw() + 0.04*getYaw();//人工修正

    // data->roll = getRoll();
    // data->pitch = getPitch();
    // data->yaw = getYaw();
}

void complementaryFilter(imu_data_t *data)
{

    // 加速度计测得的倾角
    float roll_acc = atan2f(data->accel_y, data->accel_z) * 180.0f / M_PI;
    float pitch_acc = atan2f(-data->accel_x, sqrtf(data->accel_y * data->accel_y + data->accel_z * data->accel_z)) * 180.0f / M_PI;

    // 用陀螺仪数据积分得到角速度
    float delta_roll = data->gyro_x * SAMPLE_PERIOD;
    float delta_pitch = data->gyro_y * SAMPLE_PERIOD;
    float delta_yaw = data->gyro_z * SAMPLE_PERIOD;

    // 互补滤波器更新姿态
    data->roll = 0.98f * (data->last_roll + delta_roll) + 0.02f * roll_acc;
    data->pitch = 0.98f * (data->last_pitch + delta_pitch) + 0.02f * pitch_acc;
    data->yaw = data->last_yaw + delta_yaw;

    // 更新上一次的姿态数据
    data->last_roll = data->roll;
    data->last_pitch = data->pitch;
    data->last_yaw = data->yaw;
}
